/*
 * sk_compute_trans.c
 *
 *  Created on: Feb 28, 2011
 *      Author: asish
 */
/*
 * Provides APIs and implements routines to compute the Affine Matrix and error given two sets of points.
 */
#include <math.h>
#include <stdio.h>
#include "highgui.h"
#include "cv.h"
#include "station_keeping.h"


#define WINDOW_SIZE 20
#define DIST(a,b) (sqrt((((a).x) - ((b).x))*(((a).x) - ((b).x)) + (((a).y) - ((b).y))*(((a).y) - ((b).y))))
#define DIST_P(a,b) (sqrt((((a)->x) - ((b)->x))*(((a)->x) - ((b)->x)) + (((a)->y) - ((b)->y))*(((a)->y) - ((b)->y))))
#define PI 3.14159265

static CvMat *g_affineMat = NULL;
static Deviation_t g_deviation;
static int g_iter = 0;

/*
 * Initializes the affine transformation matrix
 */
void
SK_InitAffineTrans(void)
{
	if (g_affineMat != NULL)
		cvReleaseMat(&g_affineMat);
	g_affineMat = cvCreateMat(2,3,CV_32FC1);
	memset(&g_deviation,0,sizeof(Deviation_t));
}

/*
 * Computes the angle and distance moved by the centroid given two sets of points
 */
Deviation_t *
SK_ComputeError(CvPoint2D32f *l_initialPoints,char *l_status,CvPoint2D32f *l_dstPts)
{
	int l_count = 0,i = 0;
	const float* ptr;
	double l_angle;
	double l_dist;
	double l_min;
	int mini;
	double dstAngle;
	static double srcAngle = 1000.0;
	CvPoint2D32f l_centroid1;
	CvPoint2D32f l_centroid2;

	g_iter = (g_iter + 1)%WINDOW_SIZE;

	// Compute Affine Matrix
	cvGetAffineTransform(l_initialPoints,l_dstPts,g_affineMat);

	l_centroid1 = cvPoint2D32f((l_initialPoints[0].x + l_initialPoints[1].x + l_initialPoints[2].x + l_initialPoints[3].x)/4,
								(l_initialPoints[0].y + l_initialPoints[1].y + l_initialPoints[2].y + l_initialPoints[3].y)/4);
	l_centroid2 = cvPoint2D32f((l_dstPts[0].x + l_dstPts[1].x + l_dstPts[2].x + l_dstPts[3].x)/4,
								(l_dstPts[0].y + l_dstPts[1].y + l_dstPts[2].y + l_dstPts[3].y)/4);

	l_dist = DIST(l_centroid1,l_centroid2);
	/*
	 * Ideally the distance and angle can be retrieved from the matrix, but since the edeges returned from
	 * the acoustic camera is noisy, angle is calculated by getting the principal axis.
	 */
	l_min = DIST(l_dstPts[0],l_dstPts[1]);
	mini = 1;
	for (i = 2; i<4 ; ++i)
	{
		if (DIST(l_dstPts[0],l_dstPts[i]) < l_min)
		{
			l_min = DIST(l_dstPts[0],l_dstPts[i]);
			mini = i;
		}
	}
	if (mini == 1)
	{
		CvPoint2D32f mid1 = cvPoint2D32f((l_dstPts[0].x+l_dstPts[1].x)/2,(l_dstPts[0].y+l_dstPts[1].y)/2);
		CvPoint2D32f mid2 = cvPoint2D32f((l_dstPts[2].x+l_dstPts[3].x)/2,(l_dstPts[2].y+l_dstPts[3].y)/2);
		if (fabs(mid2.x - mid1.x) > 0)
		{
			dstAngle = atan((mid2.y - mid1.y)/(mid2.x - mid1.x));
			dstAngle = dstAngle * 180 / PI; // Convert to degrees
		}
		else
		{
			dstAngle = 90;
		}

	}
	else if (mini == 2)
	{
		CvPoint2D32f mid1 = cvPoint2D32f((l_dstPts[0].x+l_dstPts[2].x)/2,(l_dstPts[0].y+l_dstPts[2].y)/2);
		CvPoint2D32f mid2 = cvPoint2D32f((l_dstPts[1].x+l_dstPts[3].x)/2,(l_dstPts[1].y+l_dstPts[3].y)/2);
		if (fabs(mid2.x - mid1.x) > 0)
		{
			dstAngle = atan((mid2.y - mid1.y)/(mid2.x - mid1.x));
			dstAngle = dstAngle * 180 / PI; // Convert to degrees
		}
		else
		{
			dstAngle = 90;
		}
	}
	else
	{
		CvPoint2D32f mid1 = cvPoint2D32f((l_dstPts[0].x+l_dstPts[3].x)/2,(l_dstPts[0].y+l_dstPts[3].y)/2);
		CvPoint2D32f mid2 = cvPoint2D32f((l_dstPts[1].x+l_dstPts[2].x)/2,(l_dstPts[1].y+l_dstPts[2].y)/2);
		if (fabs(mid2.x - mid1.x) > 0)
		{
			dstAngle = atan((mid2.y - mid1.y)/(mid2.x - mid1.x));
			dstAngle = dstAngle * 180 / PI; // Convert to degrees
		}
		else
		{
			dstAngle = 90;
		}
	}
	if (srcAngle > 999.0) // Means not initialized
	{
		l_min = DIST(l_initialPoints[0],l_initialPoints[1]);
		mini = 1;
		for (i = 2; i<4 ; ++i)
		{
			if (DIST(l_initialPoints[0],l_initialPoints[i]) < l_min)
			{
				l_min = DIST(l_initialPoints[0],l_initialPoints[i]);
				mini = i;
			}
		}
		if (mini == 1)
		{
			CvPoint2D32f mid1 = cvPoint2D32f((l_initialPoints[0].x+l_initialPoints[1].x)/2,(l_initialPoints[0].y+l_initialPoints[1].y)/2);
			CvPoint2D32f mid2 = cvPoint2D32f((l_initialPoints[2].x+l_initialPoints[3].x)/2,(l_initialPoints[2].y+l_initialPoints[3].y)/2);
			if (fabs(mid2.x - mid1.x) > 0)
			{
				srcAngle = atan((mid2.y - mid1.y)/(mid2.x - mid1.x));
				srcAngle = srcAngle * 180 / PI; // Convert to degrees
			}
			else
			{
				srcAngle = 90;
			}

		}
		else if (mini == 2)
		{
			CvPoint2D32f mid1 = cvPoint2D32f((l_initialPoints[0].x+l_initialPoints[2].x)/2,(l_initialPoints[0].y+l_initialPoints[2].y)/2);
			CvPoint2D32f mid2 = cvPoint2D32f((l_initialPoints[1].x+l_initialPoints[3].x)/2,(l_initialPoints[1].y+l_initialPoints[3].y)/2);
			if (fabs(mid2.x - mid1.x) > 0)
			{
				srcAngle = atan((mid2.y - mid1.y)/(mid2.x - mid1.x));
				srcAngle = srcAngle * 180 / PI; // Convert to degrees
			}
			else
			{
				srcAngle = 90;
			}

		}
		else
		{
			CvPoint2D32f mid1 = cvPoint2D32f((l_initialPoints[0].x+l_initialPoints[3].x)/2,(l_initialPoints[0].y+l_initialPoints[3].y)/2);
			CvPoint2D32f mid2 = cvPoint2D32f((l_initialPoints[1].x+l_initialPoints[2].x)/2,(l_initialPoints[1].y+l_initialPoints[2].y)/2);
			if (fabs(mid2.x - mid1.x) > 0)
			{
				srcAngle = atan((mid2.y - mid1.y)/(mid2.x - mid1.x));
				srcAngle = srcAngle * 180 / PI; // Convert to degrees
			}
			else
			{
				srcAngle = 90;
			}

		}
	}
	l_angle = dstAngle - srcAngle;

	if (g_iter == 0)
	{
		g_deviation.minAngle = l_angle;
		g_deviation.maxAngle = l_angle;
		g_deviation.minDisp = l_dist;
		g_deviation.maxDisp = l_dist;
	}
	else
	{
		if (l_angle > g_deviation.maxAngle)
			g_deviation.maxAngle = l_angle;
		else if (l_angle < g_deviation.minAngle)
			g_deviation.minAngle = l_angle;
		if (l_dist > g_deviation.maxDisp)
			g_deviation.maxDisp = l_dist;
		else if (l_dist < g_deviation.minDisp)
			g_deviation.minDisp = l_dist;
	}
	g_deviation.rmsAngle = sqrt((g_deviation.rmsAngle*g_deviation.rmsAngle + l_angle*l_angle)/2);
	g_deviation.rmsDisp = sqrt((g_deviation.rmsDisp*g_deviation.rmsDisp+ l_dist*l_dist)/2);

	return &g_deviation;
}

/*
 * Fetch the affine matrix
 */
CvMat *
SK_GetAffineMatrix(void)
{
	return g_affineMat;
}
